#include <Eigen/Eigen>
#include <cmath>
#include <iostream>
#include <plan_env/raycast.h>

int signum(int x) {
    return x == 0 ? 0 : x < 0 ? -1 : 1;
}

double mod(double value, double modulus) {
    return fmod(fmod(value, modulus) + modulus, modulus);
}

double intbound(double s, double ds) {
    // Find the smallest positive t such that s+t*ds is an integer.
    if (ds < 0) {
        return intbound(-s, -ds);
    } else {
        s = mod(s, 1);
        // problem is now s+t*ds = 1
        return (1 - s) / ds;
    }
}

void Raycast(const Eigen::Vector3d &start, const Eigen::Vector3d &end,
             const Eigen::Vector3d &min, const Eigen::Vector3d &max,
             int &output_points_cnt, Eigen::Vector3d *output) {
    //    std::cout << start << ' ' << end << std::endl;
    // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
    // by John Amanatides and Andrew Woo, 1987
    // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
    // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
    // Extensions to the described algorithm:
    //   • Imposed a distance limit.
    //   • The face passed through to reach the current cube is provided to
    //     the callback.

    // The foundation of this algorithm is a parameterized representation of
    // the provided ray,
    //                    origin + t * direction,
    // except that t is not actually stored; rather, at any given point in the
    // traversal, we keep track of the *greater* t values which we would have
    // if we took a step sufficient to cross a cube boundary along that axis
    // (i.e. change the integer part of the coordinate) in the variables
    // tMaxX, tMaxY, and tMaxZ.

    // Cube containing origin point.
    int x = (int)std::floor(start.x());
    int y = (int)std::floor(start.y());
    int z = (int)std::floor(start.z());
    int endX = (int)std::floor(end.x());
    int endY = (int)std::floor(end.y());
    int endZ = (int)std::floor(end.z());
    Eigen::Vector3d direction = (end - start);
    double maxDist = direction.squaredNorm();

    // Break out direction vector.
    double dx = endX - x;
    double dy = endY - y;
    double dz = endZ - z;

    // Direction to increment x,y,z when stepping.
    int stepX = (int)signum((int)dx);
    int stepY = (int)signum((int)dy);
    int stepZ = (int)signum((int)dz);

    // See description above. The initial values depend on the fractional
    // part of the origin.
    double tMaxX = intbound(start.x(), dx);
    double tMaxY = intbound(start.y(), dy);
    double tMaxZ = intbound(start.z(), dz);

    // The change in t when taking a step (always positive).
    double tDeltaX = ((double)stepX) / dx;
    double tDeltaY = ((double)stepY) / dy;
    double tDeltaZ = ((double)stepZ) / dz;

    // Avoids an infinite loop.
    if (stepX == 0 && stepY == 0 && stepZ == 0)
        return;

    double dist = 0;
    while (true) {
        if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() &&
            z >= min.z() && z < max.z()) {
            output[output_points_cnt](0) = x;
            output[output_points_cnt](1) = y;
            output[output_points_cnt](2) = z;

            output_points_cnt++;
            dist = sqrt((x - start(0)) * (x - start(0)) +
                        (y - start(1)) * (y - start(1)) +
                        (z - start(2)) * (z - start(2)));

            if (dist > maxDist)
                return;

            /*            if (output_points_cnt > 1500) {
                            std::cerr << "Error, too many racyast voxels." <<
               std::endl;
                            throw std::out_of_range("Too many raycast voxels");
                        }*/
        }

        if (x == endX && y == endY && z == endZ)
            break;

        // tMaxX stores the t-value at which we cross a cube boundary along the
        // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
        // chooses the closest cube boundary. Only the first case of the four
        // has been commented in detail.
        if (tMaxX < tMaxY) {
            if (tMaxX < tMaxZ) {
                // Update which cube we are now in.
                x += stepX;
                // Adjust tMaxX to the next X-oriented boundary crossing.
                tMaxX += tDeltaX;
            } else {
                z += stepZ;
                tMaxZ += tDeltaZ;
            }
        } else {
            if (tMaxY < tMaxZ) {
                y += stepY;
                tMaxY += tDeltaY;
            } else {
                z += stepZ;
                tMaxZ += tDeltaZ;
            }
        }
    }
}

void Raycast(const Eigen::Vector3d &start, const Eigen::Vector3d &end,
             const Eigen::Vector3d &min, const Eigen::Vector3d &max,
             std::vector<Eigen::Vector3d> *output) {
    //    std::cout << start << ' ' << end << std::endl;
    // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
    // by John Amanatides and Andrew Woo, 1987
    // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
    // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
    // Extensions to the described algorithm:
    //   • Imposed a distance limit.
    //   • The face passed through to reach the current cube is provided to
    //     the callback.

    // The foundation of this algorithm is a parameterized representation of
    // the provided ray,
    //                    origin + t * direction,
    // except that t is not actually stored; rather, at any given point in the
    // traversal, we keep track of the *greater* t values which we would have
    // if we took a step sufficient to cross a cube boundary along that axis
    // (i.e. change the integer part of the coordinate) in the variables
    // tMaxX, tMaxY, and tMaxZ.

    // Cube containing origin point.
    int x = (int)std::floor(start.x());
    int y = (int)std::floor(start.y());
    int z = (int)std::floor(start.z());
    int endX = (int)std::floor(end.x());
    int endY = (int)std::floor(end.y());
    int endZ = (int)std::floor(end.z());
    Eigen::Vector3d direction = (end - start);
    double maxDist = direction.squaredNorm();

    // Break out direction vector.
    double dx = endX - x;
    double dy = endY - y;
    double dz = endZ - z;

    // Direction to increment x,y,z when stepping.
    int stepX = (int)signum((int)dx);
    int stepY = (int)signum((int)dy);
    int stepZ = (int)signum((int)dz);

    // See description above. The initial values depend on the fractional
    // part of the origin.
    double tMaxX = intbound(start.x(), dx);
    double tMaxY = intbound(start.y(), dy);
    double tMaxZ = intbound(start.z(), dz);

    // The change in t when taking a step (always positive).
    double tDeltaX = ((double)stepX) / dx;
    double tDeltaY = ((double)stepY) / dy;
    double tDeltaZ = ((double)stepZ) / dz;

    output->clear();

    // Avoids an infinite loop.
    if (stepX == 0 && stepY == 0 && stepZ == 0)
        return;

    double dist = 0;
    while (true) {
        if (x >= min.x() && x < max.x() && y >= min.y() && y < max.y() &&
            z >= min.z() && z < max.z()) {
            output->push_back(Eigen::Vector3d(x, y, z));

            dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm();

            if (dist > maxDist)
                return;

            if (output->size() > 1500) {
                std::cerr << "Error, too many racyast voxels." << std::endl;
                throw std::out_of_range("Too many raycast voxels");
            }
        }

        if (x == endX && y == endY && z == endZ)
            break;

        // tMaxX stores the t-value at which we cross a cube boundary along the
        // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
        // chooses the closest cube boundary. Only the first case of the four
        // has been commented in detail.
        if (tMaxX < tMaxY) {
            if (tMaxX < tMaxZ) {
                // Update which cube we are now in.
                x += stepX;
                // Adjust tMaxX to the next X-oriented boundary crossing.
                tMaxX += tDeltaX;
            } else {
                z += stepZ;
                tMaxZ += tDeltaZ;
            }
        } else {
            if (tMaxY < tMaxZ) {
                y += stepY;
                tMaxY += tDeltaY;
            } else {
                z += stepZ;
                tMaxZ += tDeltaZ;
            }
        }
    }
}

bool RayCaster::setInput(const Eigen::Vector3d& start,
                         const Eigen::Vector3d& end /* , const Eigen::Vector3d& min,
                         const Eigen::Vector3d& max */) {
    start_ = start;
    end_ = end;
    // max_ = max;
    // min_ = min;

    x_ = (int)std::floor(start_.x());
    y_ = (int)std::floor(start_.y());
    z_ = (int)std::floor(start_.z());
    endX_ = (int)std::floor(end_.x());
    endY_ = (int)std::floor(end_.y());
    endZ_ = (int)std::floor(end_.z());
    direction_ = (end_ - start_);
    maxDist_ = direction_.squaredNorm();

    // Break out direction vector.
    dx_ = endX_ - x_;
    dy_ = endY_ - y_;
    dz_ = endZ_ - z_;

    // Direction to increment x,y,z when stepping.
    stepX_ = (int)signum((int)dx_);
    stepY_ = (int)signum((int)dy_);
    stepZ_ = (int)signum((int)dz_);

    // See description above. The initial values depend on the fractional
    // part of the origin.
    tMaxX_ = intbound(start_.x(), dx_);
    tMaxY_ = intbound(start_.y(), dy_);
    tMaxZ_ = intbound(start_.z(), dz_);

    // The change in t when taking a step (always positive).
    tDeltaX_ = ((double)stepX_) / dx_;
    tDeltaY_ = ((double)stepY_) / dy_;
    tDeltaZ_ = ((double)stepZ_) / dz_;

    dist_ = 0;

    step_num_ = 0;

    // Avoids an infinite loop.
    if (stepX_ == 0 && stepY_ == 0 && stepZ_ == 0)
        return false;
    else
        return true;
}

bool RayCaster::step(Eigen::Vector3d &ray_pt) {
    // if (x_ >= min_.x() && x_ < max_.x() && y_ >= min_.y() && y_ < max_.y() &&
    // z_ >= min_.z() && z_ <
    // max_.z())
    ray_pt = Eigen::Vector3d(x_, y_, z_);

    // step_num_++;

    // dist_ = (Eigen::Vector3d(x_, y_, z_) - start_).squaredNorm();

    if (x_ == endX_ && y_ == endY_ && z_ == endZ_) {
        return false;
    }

    // if (dist_ > maxDist_)
    // {
    //   return false;
    // }

    // tMaxX stores the t-value at which we cross a cube boundary along the
    // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
    // chooses the closest cube boundary. Only the first case of the four
    // has been commented in detail.
    if (tMaxX_ < tMaxY_) {
        if (tMaxX_ < tMaxZ_) {
            // Update which cube we are now in.
            x_ += stepX_;
            // Adjust tMaxX to the next X-oriented boundary crossing.
            tMaxX_ += tDeltaX_;
        } else {
            z_ += stepZ_;
            tMaxZ_ += tDeltaZ_;
        }
    } else {
        if (tMaxY_ < tMaxZ_) {
            y_ += stepY_;
            tMaxY_ += tDeltaY_;
        } else {
            z_ += stepZ_;
            tMaxZ_ += tDeltaZ_;
        }
    }

    return true;
}